/**
* Copyright 2020 Huawei Technologies Co., Ltd
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at

* http://www.apache.org/licenses/LICENSE-2.0

* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.

* File utils.cpp
* Description: handle file operations
*/
#include "utils.h"
#include <bits/stdint-uintn.h>
#include <map>
#include <iostream>
#include <fstream>
#include <string>
#include <unistd.h>
#include <cstring>
#include <dirent.h>
#include <vector>
#include <string.h>
#include <sys/stat.h>
#include <sys/types.h>
#include "acl/acl.h"
//#include "acl/ops/acl_dvpp.h"

using namespace std;

int limbSeq[17][2] = {{2, 3}, {2, 6}, {3, 4}, {4, 5}, {6, 7}, {7, 8}, {2, 9}, {9, 10},
{10, 11}, {2, 12}, {12, 13}, {13, 14}, {2, 1}, {1, 15}, {15, 17},
{1, 16}, {16, 18}};

cv::Scalar colors[18] = {cv::Scalar(255, 0, 0), cv::Scalar(255, 85, 0), cv::Scalar(255, 170, 0), cv::Scalar(255, 255, 0), cv::Scalar(170, 255, 0), cv::Scalar(85, 255, 0), cv::Scalar(0, 255, 0),
cv::Scalar(0, 255, 85), cv::Scalar(0, 255, 170), cv::Scalar(0, 255, 255), cv::Scalar(0, 170, 255), cv::Scalar(0, 85, 255), cv::Scalar(0, 0, 255), cv::Scalar(85, 0, 255),
cv::Scalar(170, 0, 255), cv::Scalar(255, 0, 255), cv::Scalar(255, 0, 170), cv::Scalar(255, 0, 85)};

namespace {
const std::string kImagePathSeparator = ",";
const int kStatSuccess = 0;
const std::string kFileSperator = "/";
const std::string kPathSeparator = "/";
// output image prefix
const std::string kOutputFilePrefix = "out_";

}

bool Utils::IsDirectory(const string &path) {
    // get path stat
    struct stat buf;
    if (stat(path.c_str(), &buf) != kStatSuccess) {
        return false;
    }

    // check
    if (S_ISDIR(buf.st_mode)) {
        return true;
    } else {
    return false;
    }
}

bool Utils::IsPathExist(const string &path) {
    ifstream file(path);
    if (!file) {
        return false;
    }
    return true;
}

void Utils::SplitPath(const string &path, vector<string> &path_vec) {
    char *char_path = const_cast<char*>(path.c_str());
    const char *char_split = kImagePathSeparator.c_str();
    char *tmp_path = strtok(char_path, char_split);
    while (tmp_path) {
        path_vec.emplace_back(tmp_path);
        tmp_path = strtok(nullptr, char_split);
    }
}

void Utils::GetAllFiles(const string &path, vector<string> &file_vec) {
    // split file path
    vector<string> path_vector;
    SplitPath(path, path_vector);

    for (string every_path : path_vector) {
        // check path exist or not
        if (!IsPathExist(path)) {
        ERROR_LOG("Failed to deal path=%s. Reason: not exist or can not access.",
                every_path.c_str());
        continue;
        }
        // get files in path and sub-path
        GetPathFiles(every_path, file_vec);
    }
}

void Utils::GetPathFiles(const string &path, vector<string> &file_vec) {
    struct dirent *dirent_ptr = nullptr;
    DIR *dir = nullptr;
    if (IsDirectory(path)) {
        dir = opendir(path.c_str());
        while ((dirent_ptr = readdir(dir)) != nullptr) {
            // skip . and ..
            if (dirent_ptr->d_name[0] == '.') {
            continue;
            }

            // file path
            string full_path = path + kPathSeparator + dirent_ptr->d_name;
            // directory need recursion
            if (IsDirectory(full_path)) {
                GetPathFiles(full_path, file_vec);
            } else {
                // put file
                file_vec.emplace_back(full_path);
            }
        }
    } 
    else {
        file_vec.emplace_back(path);
    }
}

void* Utils::CopyDataDeviceToLocal(void* deviceData, uint32_t dataSize) {
    uint8_t* buffer = new uint8_t[dataSize];
    if (buffer == nullptr) {
        ERROR_LOG("New malloc memory failed");
        return nullptr;
    }

    aclError aclRet = aclrtMemcpy(buffer, dataSize, deviceData, dataSize, ACL_MEMCPY_DEVICE_TO_HOST);
    if (aclRet != ACL_ERROR_NONE) {
        ERROR_LOG("Copy device data to local failed, aclRet is %d", aclRet);
        delete[](buffer);
        return nullptr;
    }

    return (void*)buffer;
}

void* Utils::CopyDataToDevice(void* data, uint32_t dataSize, aclrtMemcpyKind policy) {
    void* buffer = nullptr;
    aclError aclRet = aclrtMalloc(&buffer, dataSize, ACL_MEM_MALLOC_HUGE_FIRST);
    if (aclRet != ACL_ERROR_NONE) {
        ERROR_LOG("malloc device data buffer failed, aclRet is %d", aclRet);
        return nullptr;
    }

    aclRet = aclrtMemcpy(buffer, dataSize, data, dataSize, policy);
    if (aclRet != ACL_ERROR_NONE) {
        ERROR_LOG("Copy data to device failed, aclRet is %d", aclRet);
        (void)aclrtFree(buffer);
        return nullptr;
    }

    return buffer;
}

void* Utils::CopyDataDeviceToDevice(void* deviceData, uint32_t dataSize) {
    return CopyDataToDevice(deviceData, dataSize, ACL_MEMCPY_DEVICE_TO_DEVICE);
}

void* Utils::CopyDataHostToDevice(void* deviceData, uint32_t dataSize) {
    return CopyDataToDevice(deviceData, dataSize, ACL_MEMCPY_HOST_TO_DEVICE);
}

void Utils::WriteMotionData(float motion_data[1][3][FRAME_LENGTH][18])
{
    // write motion_data array into a txt file, in NHWC format
    static int dataid=1;
    cout<<"writing motion data..."<<"id: "<<dataid<<endl;
    ofstream fout;
    string filename="./output/motion_data_";
    filename+=to_string(dataid)+".txt";
    fout.open(filename);
    fout.setf(ios::fixed);
    fout.precision(8);

    for(int j=0;j<FRAME_LENGTH;j++)
    {
        for(int k=0;k<18;k++)
        {
            for(int i=0;i<3;i++)
                fout<<motion_data[0][i][j][k]<<" "; // NHWC format
        }

    }

//    for(int i=0;i<3;i++)
//    {
//        for(int j=0;j<FRAME_LENGTH;j++)
//        {
//            for(int k=0;k<18;k++)
//            {
//                fout<<motion_data[0][i][j][k]<<" "; // NCHW format
//            }
//
//        }
//    }
//    cout<<"write motion data success!"<<endl;
    dataid++;

}

void Utils::write_array_data(float* array,int n,string name)
{
    ofstream fout;
    string filename="./output/"+name+".txt";
    fout.open(filename);
    fout.setf(ios::fixed);
    fout.precision(8);
    for(int i=0;i<n;i++)
    {
        fout<<array[i]<<" ";
    }

}


void Utils::ProcessMotionData(float motion_data[1][3][FRAME_LENGTH][18]) {
    // normalize the motion_data, i.e. keypoints data

    for(int i=0;i<2;i++) {
        for(int j=0;j<FRAME_LENGTH;j++) {
            for(int k=0;k<18;k++)
            {
                if(i==0)
                    motion_data[0][i][j][k]=motion_data[0][i][j][k]/modelWidth_-0.5;
                else
                    motion_data[0][i][j][k]=-0.5+motion_data[0][i][j][k]/modelHeight_;
            }
        }

    }

}

void Utils::PresentKeyPoints(string imagePath,vector<key_pointsT> all_keypoints)
{
    // draw human keypoints into an image from the given path and save it
    static int id=0;
    cv::Mat img=cv::imread(imagePath,CV_32FC3);
    float col_factor=img.cols/modelWidth_;
    float row_factor=img.rows/modelHeight_;

    for(int i=0;i<all_keypoints.size();i++)
    {
        key_pointsT kpt=all_keypoints[i];
        cv::Point p(col_factor*kpt.x,row_factor*kpt.y);
        cv::circle(img, p, kpt.score*2, cv::Scalar(0, 255, 255));
    }
    cv::imwrite("./output/result"+to_string(id)+".png", img);
    id++;
}

void Utils::PresentKeyPoints(cv::Mat& frame,vector<key_pointsT> all_keypoints)
{
    // draw human skeleton keypoints into an opencv image
    float col_factor=frame.cols/modelWidth_;
    float row_factor=frame.rows/modelHeight_;

    for(int i=0;i<all_keypoints.size();i++)
    {
        key_pointsT kpt=all_keypoints[i];
        cv::Point p(col_factor*kpt.x,row_factor*kpt.y);
        cv::circle(frame, p, kpt.score*2, colors[i]);
    }
}


void Utils::DrawHuman(string imagePath,vector<key_pointsT> all_keypoints)
{
    // draw human skeletonn from image path and Openpose inferenced keypoints
    static int id=0;
    cv::Mat ori = cv::imread(imagePath,CV_32FC3);
    float col_factor=ori.cols/modelWidth_;
    float row_factor=ori.rows/modelHeight_;
    for(int i=0;i<all_keypoints.size();i++)
    {
        key_pointsT kpt=all_keypoints[i];
        cv::Point p(col_factor*kpt.x,row_factor*kpt.y);
        if(kpt.score==0) continue;
        cv::circle(ori, p, kpt.score*6, colors[i],1);
    }
    for(int i=0;i<17;i++){
        int k1=limbSeq[i][0]-1;
        int k2=limbSeq[i][1]-1;
        if(all_keypoints[k1].score==0) continue;
        if(all_keypoints[k2].score==0) continue;
        cv::Point p1(col_factor*all_keypoints[k1].x,row_factor*all_keypoints[k1].y);
        cv::Point p2(col_factor*all_keypoints[k2].x,row_factor*all_keypoints[k2].y);
        cv::line(ori,p1,p2,colors[i],4);
    }

    cv::imwrite("./output/human_result_"+to_string(id)+".png", ori);
    id++;
}

void Utils::DrawHuman(cv::Mat& ori,vector<key_pointsT> all_keypoints)
{
    // draw human skeleton from opencv image matrix and Openpose inferenced keypoints
    float col_factor=ori.cols/modelWidth_;
    float row_factor=ori.rows/modelHeight_;
    for(int i=0;i<18;i++)
    {
        key_pointsT kpt=all_keypoints[i];
        cv::Point p(col_factor*kpt.x,row_factor*kpt.y);
        if(kpt.score==0) continue;
        cv::circle(ori, p, kpt.score*6, colors[i],-1); // -1:filled
    }
    for(int i=0;i<17;i++){
        int k1=limbSeq[i][0]-1;
        int k2=limbSeq[i][1]-1;
        if(all_keypoints[k1].score==0) continue;
        if(all_keypoints[k2].score==0) continue;
        cv::Point p1(col_factor*all_keypoints[k1].x,row_factor*all_keypoints[k1].y);
        cv::Point p2(col_factor*all_keypoints[k2].x,row_factor*all_keypoints[k2].y);
        cv::line(ori,p1,p2,colors[i],4);
    }
}


void Utils::DrawHuman(cv::Mat& ori,float motion_data[1][3][FRAME_LENGTH][18],int frame_id)
{
    // draw single frame from motion_data
    float col_factor=ori.cols/modelWidth_;
    float row_factor=ori.rows/modelHeight_;
    for(int i=0;i<18;i++)
    {
        // recover from normalized data
        float x=modelWidth_*(motion_data[0][0][frame_id][i]+0.5);
        float y=modelHeight_*(0.5-motion_data[0][1][frame_id][i]);
        float s=motion_data[0][2][frame_id][i];
        cout<<"x: "<<x<<" y: "<<y<<" s: "<<s<<endl;
        cv::Point p(col_factor*x,row_factor*y);
        if(s==0) continue;
        cv::circle(ori,p,s*6, colors[i],-1);

    }

    for(int i=0;i<17;i++){
        int k1=limbSeq[i][0]-1;
        int k2=limbSeq[i][1]-1;
        float x1=modelWidth_*(motion_data[0][0][frame_id][k1]+0.5);
        float x2=modelWidth_*(motion_data[0][0][frame_id][k2]+0.5);
        float y1=modelHeight_*(0.5-motion_data[0][1][frame_id][k1]);
        float y2=modelHeight_*(0.5-motion_data[0][1][frame_id][k2]);
        float s1=motion_data[0][2][frame_id][k1];
        float s2=motion_data[0][2][frame_id][k2];

        if(s1==0||s2==0) continue;
        cv::Point p1(col_factor*x1,row_factor*y1);
        cv::Point p2(col_factor*x2,row_factor*y2);
        cv::line(ori,p1,p2,colors[i],4);
    }

}

void Utils::DrawHuman(float motion_data[1][3][FRAME_LENGTH][18])
{
    // draw frames from motion_data and construct a video
    static int id=1;
    int cols=1280;
    int rows=720;
    int fps = 15;
    cv::VideoWriter outputVideo;
    outputVideo.open("./output/motion"+to_string(id)+".avi",cv::VideoWriter::fourcc('M','J','P','G'),fps, cv::Size(cols,rows));
//    cout<<"is open: "<<outputVideo.isOpened()<<endl;
    float col_factor=(float)cols/modelWidth_;
    float row_factor=(float)rows/modelHeight_;

    for(int t=0;t<FRAME_LENGTH;t++)
    {
        cv::Mat ori(rows,cols, CV_8UC3, cv::Scalar(0,0,0));
        cout<<"construct ori success"<<" t:"<<t<<endl;
        for(int i=0;i<18;i++)
        {
            // recover from normalized data
            float x=modelWidth_*(motion_data[0][0][t][i]+0.5);
            float y=modelHeight_*(0.5-motion_data[0][1][t][i]);
            float s=motion_data[0][2][t][i];

            //        cout<<"x: "<<x<<" y: "<<y<<" s: "<<s<<endl;
            cv::Point p(col_factor*x,row_factor*y);
            if(s==0) continue;
            cv::circle(ori,p,s*5, colors[i],-1);
        }

        for(int i=0;i<17;i++){
            int k1=limbSeq[i][0]-1;
            int k2=limbSeq[i][1]-1;
            float x1=modelWidth_*(motion_data[0][0][t][k1]+0.5);
            float x2=modelWidth_*(motion_data[0][0][t][k2]+0.5);
            float y1=modelHeight_*(0.5-motion_data[0][1][t][k1]);
            float y2=modelHeight_*(0.5-motion_data[0][1][t][k2]);
            float s1=motion_data[0][2][t][k1];
            float s2=motion_data[0][2][t][k2];

            if(s1==0||s2==0) continue;
            cv::Point p1(col_factor*x1,row_factor*y1);
            cv::Point p2(col_factor*x2,row_factor*y2);
            cv::line(ori,p1,p2,colors[i],4);
        }
//        cv::imwrite("./output/"+to_string(t)+".jpg",ori);
        outputVideo<<ori;

    }
    outputVideo.release();
    id++;

}

void Utils::read_motion_data(string filename,float motion_data[1][3][FRAME_LENGTH][18],bool mode)
{
    // read motion_data_*.txt file into an array of NCHW format
    // mode == true: read NHWC data, false: NCHW data
    ifstream fin;
    fin.open(filename);
    if(mode==true)
    {
        for(int j=0;j<FRAME_LENGTH;j++)
        {
            for(int k=0;k<18;k++)
            {
                for (int i = 0; i < 3; i++)
                {
                    fin>>motion_data[0][i][j][k];
                }
            }
        }
    }

    else
    {
        for (int i = 0; i < 3; i++)
        {
            for(int j=0;j<FRAME_LENGTH;j++)
            {
                for(int k=0;k<18;k++)
                {
                    fin>>motion_data[0][i][j][k];
                    //                cout<<motion_data[0][i][j][k]<<" ";
                }
            }
        }
    }

}
void Utils::test()
{
    // for debugging
    string motion_path="../../GestureDetect_e8aa832f/out/output/motion_data_0.txt";
    int image_id=0;
    string frame_path="../../GestureDetect_e8aa832f/data/frames/"+to_string(image_id)+".jpg";  //"../data/demo/swiping_down/1/"+to_string(image_id)+".png";
    cv::Mat frame=cv::imread(frame_path);
    float motion_data[1][3][FRAME_LENGTH][18];
    Utils::read_motion_data(motion_path,motion_data,false);
    Utils::DrawHuman(frame,motion_data,image_id);
    cv::imwrite("./output/human.jpg",frame);

}
